﻿#include "InspectLL.h"

#include <QApplication>
#include <QJsonDocument>
#include <QJsonObject>
#include <QJsonArray>
#include <QJsonValue>
#include <QFile>

#include <QDateTime>
#include "VisionMainWnd.h"
#include "Performance.h"
#include "sql_data.h"
#include <QAbstractEventDispatcher>
#include <QThread>
#include <QTimer>
#include <limits>
#include <QDir>
#include <random>

#include "Visionapi.h"
#include "Calibration_NinePoint.h"
using namespace HalconCpp;

//#include "IInfer.h"
//#include "InferObjDetect.h"

InspectLL::InspectLL()
{
    setDrawAllResult(false);
    SetSystem("border_shape_models", "true");

}

InspectLL::~InspectLL()
{
    delete calibration_;
    calibration_ = nullptr;

    //delete infer_objDetect_;
    //infer_objDetect_ = nullptr;

    saveRoi();
}

void InspectLL::grabImage(cv::Mat& img)
{
}

void InspectLL::inspect(cv::Mat& src)
{
    Performance timeTest;
    timeTest.Reset();

    setFindObj(false);
    setFinished(false);

    HImage srcImg = VisionApi::HImageFromMat(src);
    
    HImage roiImg = preProcess(srcImg);
    resetMatchCount();
    allResults_.clear();

    HTuple modelIds;
    HTuple r, c, a, s, score;
    //roiImg.FindScaledShapeModels(mulShapeModel_, HTuple(0).TupleRad().D(), HTuple(360).TupleRad().D(),
    //    0.8, 1.2, 0.55, 0, 0.3, HTuple("least_squares").S(),
    //    HTuple("auto"), 0.9, &r, &c, &a, &s, &score, &modelIds);
    

    FindScaledShapeModels(roiImg, mulShapeModels_, -(HTuple(180).TupleRad()), HTuple(360).TupleRad(),
        0.8, 1.2, 0.7, 0, 0.5, "least_squares",
        0, 0.9, &r, &c, &a, &s, &score, &modelIds);



    // 记录匹配物料数目，以及目标的姿态保存到队列
    matchCount_ = r.Length();
    for (int i = 0; i < matchCount_; i++)
    {
        MatchResult mr;
        mr.index = i;
        mr.x = c[i].D();
        mr.y = r[i].D();
        mr.score = score[i].D();
        mr.rotAngle = a[i].D();
        mr.sx = mr.sy = s[i].D();
        mr.modelId = modelIds[i].I();
        
        allResults_.push_back(mr);
    }

    //bestResult_ = findBestResult();   
    bestResult_ = findBestResult_farPriority();
    if (bestResult_)
    {
        calibration_->affineTransToRobot(bestResult_->x, bestResult_->y);
        setFindObj(true);
        addObjCount();
    }

    // 绘制结果：可通过设置绘制所有结果，或者只绘制最佳结果
    //drawAllResult(src);   
    drawResultsRect2(src);

    // 将目标旋转角度统一到：[-90° 90°]
    for (auto& m : allResults_)
    {
        m.rotAngle = VisionApi::rad2Deg(m.rotAngle);
        while (m.rotAngle>90)
        {

             m.rotAngle -= 180;
       
        }       
        while (m.rotAngle < -90)
        {
             m.rotAngle += 180;
        }

        m.rotAngle *= -1;
    }
    // test ai infer
    //infer_objDetect_->infer(src);
    //VisionApi::drawInferResults(src, infer_objDetect_->getInferResult()->getResults());

    setFinished(true);

    inspectTime_ = timeTest.GetTimeMilli();

}

cv::Point2d InspectLL::getImageXY()
{
    return cv::Point2d();
}

cv::Point2d InspectLL::getRobotXY()
{
    return calibration_->getObjRobotPos();
}

void InspectLL::saveRoi()
{
    // 模板目录
    QString configPath = QApplication::applicationDirPath() + "/config/alg.json";

    QFile file(configPath);
    QByteArray jsonData;
    if (file.open(QIODevice::ReadOnly)) {
        jsonData = file.readAll();
        file.close();
    }
    QJsonParseError err_rpt;
    QJsonDocument jsonDocu = QJsonDocument::fromJson(jsonData, &err_rpt);
    if (jsonDocu.isObject())
    {
        auto rootObj = jsonDocu.object();
        auto inspObj = rootObj["inspect"].toObject();
        
        // 坐标数组保存：roi
        QVector<QVector<int>> rectPoints = {
        {int(searchRoi_.x), int(searchRoi_.y)},
        {int(searchRoi_.br().x), int(searchRoi_.br().y)}
        };
        // 创建根数组
        QJsonArray rootArray;
        // 填充坐标数据
        for (const auto& point : rectPoints) {
            QJsonArray pointArray;
            pointArray.append(point[0]);
            pointArray.append(point[1]);
            rootArray.append(pointArray);
        }
        auto region = inspObj["region_roi"].toObject(); 
        region["rect_roi"] = rootArray;

        inspObj["region_roi"] = region;

        rootObj["inspect"] = inspObj;
        jsonDocu.setObject(rootObj);

    }

    QFile fileSave(configPath);
    if (fileSave.open(QIODevice::ReadWrite | QIODevice::Truncate)) {
        fileSave.write(jsonDocu.toJson());
        fileSave.close();
    }
}

void InspectLL::init()
{
    readCalibParams();
    loadConfig();
    loadModel();  

    // temp
    //infer_objDetect_ = new InferObjDetect();
    //infer_objDetect_->setParams(0.25, 0.25, 0.45);
    //infer_objDetect_->loadModelFile("best.onnx");
    //infer_objDetect_->loadLabelFile("data-connector.yaml");

}

void InspectLL::loadModel()
{
    if (!lastPn_.isEmpty())
    {
        loadModel(lastPn_.toStdString().c_str());
    }    
}

void InspectLL::loadConfig()
{
    // 模板目录
    QString configPath = QApplication::applicationDirPath() + "/config/alg.json";

    QFile file(configPath);
    QByteArray jsonData;
    if (file.open(QIODevice::ReadOnly)) {
        jsonData = file.readAll();
        file.close();
    }
    QJsonParseError err_rpt;
    QJsonDocument jsonDocu = QJsonDocument::fromJson(jsonData, &err_rpt);
    if (jsonDocu.isObject())
    {
        auto rootObj = jsonDocu.object();
        auto inspObj = rootObj["inspect"].toObject();
        lastPn_ = inspObj["last_pn"].toString();
        auto roiObj = inspObj["region_roi"].toObject();
        auto roiPoints = roiObj["rect_roi"].toArray();
        int n = roiPoints.size();

        cv::Point2d ltPoint, brPoint;
        auto p1 = roiPoints[0].toArray();
        ltPoint.x = p1.at(0).toDouble();
        ltPoint.y = p1.at(1).toDouble();
        auto p2 = roiPoints[1].toArray();
        brPoint.x = p2.at(0).toDouble();
        brPoint.y = p2.at(1).toDouble();
        searchRoi_.x = ltPoint.x;
        searchRoi_.y = ltPoint.y;
        searchRoi_.width = brPoint.x - ltPoint.x;
        searchRoi_.height = brPoint.y - ltPoint.y;

        roiVisible_ = roiObj["visible"].toBool();
        //roiColor_ = roiObj["color"].toArray();    // 加载显示颜色

        HalconCpp::GenRectangle1(&matchRoi_, ltPoint.y, ltPoint.x, brPoint.y, brPoint.x);
    }
    else
    {
        qInfo() << "模板加载失败";
    }
}

void InspectLL::loadModel(QString dirPN)
{
    // 模板目录
    QString modelDir = QApplication::applicationDirPath() + "/model/" + dirPN;

    QStringList allShapeModelFile;
    QDir dir(modelDir); // 替换为你的目录路径
    QStringList files = dir.entryList(QDir::Files); // 只获取文件列表
    foreach(const QString & file, files) {
        if (file.right(4) == ".shm")
        {
            allShapeModelFile.append(file);
        }
    }

    if (allShapeModelFile.size() < 1)
    {
        qInfo() << "模板文件加载失败，请检查目录文件是否存在";
        return;
    }
    mulShapeModels_.Clear();
    mulShapeModel_.Clear();
    modelContourRect2_.clear();
    HTuple allModelId;
    for (auto& sh : allShapeModelFile)
    {
        QString modelPath = modelDir + "//"+sh;

        // 计算模板最小外接矩形
        HTuple modelId;
        HalconCpp::ReadShapeModel(modelPath.toStdString().c_str(), &modelId);
        HXLDCont modelContour;
        HalconCpp::GetShapeModelContours(&modelContour, modelId, 1);
        //HXLDCont selXld;
        //HalconCpp::SelectShapeXld(modelContour, &selXld, "con")
        HTuple r, c, phi, len1, len2;
        HalconCpp::SmallestRectangle2Xld(modelContour, &r, &c, &phi, &len1, &len2);

        RotRect rect2 = { r, c, phi, len1, len2 };
        modelContourRect2_.push_back(rect2);

        if (modelId.Length() == 0)
        {
            qInfo() << "模板加载失败";
            return;
        }
        else
        {
            qInfo() << sh + "-加载成功";
        }

        //mulShapeModel_.SetFromTuple(modelId);
        allModelId.Append(modelId);
        mulShapeModels_.Append(modelId);
    }

    mulShapeModel_.SetFromTuple(allModelId);
}

void InspectLL::saveModelInfo()
{
    // 模板目录
    QString configPath = QApplication::applicationDirPath() + "/config/alg.json";

    QFile file(configPath);
    QByteArray jsonData;
    if (file.open(QIODevice::ReadOnly)) {
        jsonData = file.readAll();
        file.close();
    }
    QJsonParseError err_rpt;
    QJsonDocument jsonDocu = QJsonDocument::fromJson(jsonData, &err_rpt);
    if (jsonDocu.isObject())
    {
        auto rootObj = jsonDocu.object();
        auto inspObj = rootObj["inspect"].toObject();
        inspObj["last_pn"] = lastPn_;
        rootObj["inspect"] = inspObj;
        jsonDocu.setObject(rootObj);

    }

    QFile fileSave(configPath);
    if (fileSave.open(QIODevice::ReadWrite | QIODevice::Truncate)) {
        fileSave.write(jsonDocu.toJson());
        fileSave.close();
    }
}

MatchResult* InspectLL::findBestResult()
{
    MatchResult* r = nullptr;
    if (allResults_.size() == 1)
    {
        r = &allResults_[0];
    }
    else if (allResults_.size() > 1)
    {
        r = &allResults_[0];
        for (auto& m : allResults_)
        {
            if (m.score > r->score)
            {
                r = &m;
            }
        }
    }

    return r;
}

MatchResult* InspectLL::findBestResult_farPriority()
{
    MatchResult* r = nullptr;
    if (allResults_.size() == 1)
    {
        r = &allResults_[0];
    }
    else if (allResults_.size() > 1)
    {
        r = &allResults_[0];
        for (auto& m : allResults_)
        {
            if (m.x < r->x)
            {
                r = &m;
            }
        }
    }
    return r;
}

void InspectLL::readParams()
{
    QJsonDocument readAlg;
    QString strPath = QCoreApplication::applicationDirPath() + "/config/alg.json";
    QFile file(strPath);
    QByteArray jsonData;
    if (file.open(QIODevice::ReadOnly)) {
        jsonData = file.readAll().trimmed();
        file.close();
    }
    QJsonParseError err_rpt;
    QJsonDocument jsonDocu = QJsonDocument::fromJson(jsonData, &err_rpt);
    if (jsonDocu.isObject())
    {
        auto inferParams = jsonDocu["yolo"];
        std::string modelFile = inferParams["model_file"].toString().toStdString();
        std::string classesFile = inferParams["classes_label"].toString().toStdString();
        double confThresh = inferParams["confidence_thresh"].toDouble();
        double nmsThresh = inferParams["nms_thresh"].toDouble();
        double scoreThresh = inferParams["score_thresh"].toDouble();

        // 读取警戒区域：报警，停机
        auto alg = jsonDocu["inspect"];
        auto rAlarm = alg["region_alarm"];
        auto color = rAlarm["color"].toArray();
    }
}

void InspectLL::readCalibParams()
{
    calibration_ = new Calibration_NinePoint();   

    QJsonDocument readAlg;
    QString strPath = QCoreApplication::applicationDirPath() + "/calibration/calib_config.json";
    QFile file(strPath);
    QByteArray jsonData;
    if (file.open(QIODevice::ReadOnly)) {
        jsonData = file.readAll().trimmed();
        file.close();
    }
    QJsonParseError err_rpt;
    QJsonDocument jsonDocu = QJsonDocument::fromJson(jsonData, &err_rpt);
    if (jsonDocu.isObject())
    {
        auto calibParams = jsonDocu["calib_params"];
        QString calibFile = calibParams["calib_file"].toString();
        QString calibType = calibParams["nine_point"].toString();
        
        QString strPath = QApplication::applicationDirPath() + "/calibration/"+calibFile;
        calibration_->readTransFile(strPath.toStdString().c_str());
        qInfo() << "read calibFile success: " << strPath;
    }
}

// 处理检测线程
void InspectLL::loopProcess()
{
    cv::Mat src;
    long count_image = 0;
    Performance timeTest;

    while (true)
    {
        timeTest.Reset();

        // 1.判断采集图像队列数目：数目为0直接返回
        // 图像有效性判断


        // 2.推理检测
        // 2.1 清空信息输出链表

        // 3. 处理结果显示：保存报警图像到icon列表

        // 4.从缓存队列删除头节点图像

        // 5. 信息输出

        count_image++;
    }

}
bool InspectLL::findMaxMatchIndex(HTuple &scores)
{
    bool findSuc = true;
    matchMaxScore_ = 0;
    indexOfMaxScore_ = -1;
    int n = scores.Length();
    if (n > 1)
    {
        matchMaxScore_ = scores.TupleMax();
        indexOfMaxScore_ = scores.TupleFind(matchMaxScore_);
    }
    else if(n==1)
    {
        matchMaxScore_ = scores.D();
        indexOfMaxScore_ = 0;
    }
    else
    {
        findSuc = false;
    }
    return findSuc;
}

void InspectLL::drawResult(cv::Mat& src, std::vector<polygonPoints>& allContours)
{
    for (auto& ply : allContours)
    {
        std::random_device rd;
        std::mt19937 gen(rd());
        std::uniform_int_distribution<int> dis(100, 255);
        auto color = cv::Scalar(dis(gen),
            dis(gen),
            dis(gen));
        std::vector<cv::Point> points;
        for (auto& p : ply)
        {
            points.push_back(p);
        }
        cv::polylines(src, points,false, color, 1, cv::LINE_AA, 0);
    }
}

HImage InspectLL::preProcess(HImage& src)
{
    //modelImg = modelImg.ReduceDomain(matchRoi_);
    src = src.MedianImage("circle", 1.5, "mirrored");
    matchRoi_.GenRectangle1(searchRoi_.tl().y, searchRoi_.tl().x, searchRoi_.br().y, searchRoi_.br().x);
    HImage roiImg = src.ReduceDomain(matchRoi_);
    return roiImg;
}

HRegion InspectLL::easySegmentObject(HImage src, double lowThres, double highThres)
{
    HRegion objRegion = src.Threshold(lowThres, highThres);
    objRegion = objRegion.OpeningCircle(3.5);
    objRegion = objRegion.ClosingCircle(5.5);

    objRegion = objRegion.SelectShape("area", "and", 200, 999999);
    return objRegion;
}

void InspectLL::drawAllResult(cv::Mat& src)
{
    cv::Mat destImg = src;
    if (bestResult_)
    {
        cv::cvtColor(destImg, destImg, cv::COLOR_GRAY2BGR);
    }

    for (auto& r : allResults_)
    {
        if (!isDrawAllResult() && r.index != bestResult_->index)
        {
            continue;
        }

        int id = r.modelId;
        std::vector<HXLDCont> allContours;
        HXLDCont modelContours;
        HalconCpp::GetShapeModelContours(&modelContours, mulShapeModel_.ConvertToTuple()[id], 1);
        

        // 计算变换矩阵
        //int id = bestResult_->modelId;
        HXLDCont transXld;
        HHomMat2D homMat;
        homMat.HomMat2dIdentity();
        homMat = homMat.HomMat2dTranslate(r.y, r.x);
        homMat = homMat.HomMat2dRotate(r.rotAngle, r.y, r.x);
        homMat = homMat.HomMat2dScale(r.sx, r.sy, r.y, r.x);

        // 将halcon轮廓转为opencv轮廓点
        HalconCpp::AffineTransContourXld(modelContours, &transXld, homMat);

        // 随机取色
        std::random_device rd;
        std::mt19937 gen(rd());
        std::uniform_int_distribution<int> dis(100, 255);
        cv::Scalar color = cv::Scalar(dis(gen),
            dis(gen),
            dis(gen));

        // 最佳的固定为绿色显示
        if (bestResult_->index==r.index)
        {
            color = cv::Scalar(0,255,0);
        }

        // 绘制
        VisionApi::drawHalconXld(destImg, transXld, color);

        // 绘制中心点
        double r1 = 0, c1 = 0;
        HTuple outRow, outCol;
        HalconCpp::AffineTransPixel(homMat, r1, c1, &outRow, &outCol);
        cv::Point pt(outCol.D(), outRow.D());
        VisionApi::drawCross(destImg, pt);        
    }    
    // 保存到原图像
    src = destImg;
}

void InspectLL::drawResultsRect2(cv::Mat& src)
{
    cv::Mat destImg = src;
    if (bestResult_)
    {
        cv::cvtColor(destImg, destImg, cv::COLOR_GRAY2BGR);
    }

    for (auto& r : allResults_)
    {
        if (!isDrawAllResult() && r.index != bestResult_->index)
        {
            continue;
        }

        int id = r.modelId;
        HXLDCont transXld;
        HHomMat2D homMat;
        homMat.HomMat2dIdentity();
        homMat = homMat.HomMat2dTranslate(r.y, r.x);
        homMat = homMat.HomMat2dRotate(r.rotAngle, r.y, r.x);
        homMat = homMat.HomMat2dScale(r.sx, r.sy, r.y, r.x);
        
        RotRect rect2 = modelContourRect2_.at(id);
        rect2.x += r.x;
        rect2.y += r.y;
        rect2.angle += r.rotAngle;

        // 随机取色
        std::random_device rd;
        std::mt19937 gen(rd());
        std::uniform_int_distribution<int> dis(100, 255);
        cv::Scalar color = cv::Scalar(dis(gen),
            dis(gen),
            dis(gen));

        // 最佳的固定为绿色显示
        if (bestResult_->index == r.index)
        {
            color = cv::Scalar(0, 255, 0);
        }

        // 绘制
        //VisionApi::drawHalconXld(destImg, transXld, color);
        VisionApi::drawRotRect(destImg, rect2, color);

        // 绘制中心点
        double r1 = 0, c1 = 0;
        HTuple outRow, outCol;
        HalconCpp::AffineTransPixel(homMat, r1, c1, &outRow, &outCol);
        cv::Point pt(outCol.D(), outRow.D());
        VisionApi::drawCross(destImg, pt);
    }
    // 保存到原图像
    src = destImg;
}
